#include "tocUtil.h"

using namespace std;

void drawTriangle(unsigned int look, float x, float y, 
		  float halfLength, float halfBreadth, float e) 
{
	glPushMatrix();
	glBegin(GL_TRIANGLES);
	switch(look) {
	case NORTH:
		glVertex2f(x, y + (halfLength + e));
		glVertex2f(x - (halfBreadth + e), 
			   y - (halfBreadth + e));
		glVertex2f(x + (halfBreadth + e), 
			   y - (halfBreadth + e));
		break;

	case SOUTH:
		glVertex2f(x, y - (halfLength + e));
		glVertex2f(x - (halfBreadth + e), 
			   y + (halfBreadth + e));
		glVertex2f(x + (halfBreadth + e), 
			   y + (halfBreadth + e));
		break;

	case EAST:
		glVertex2f(x + (halfLength + e), y);
		glVertex2f(x - (halfBreadth + e), 
			   y - (halfBreadth + e));
		glVertex2f(x - (halfBreadth + e), 
			   y + (halfBreadth + e));
		break;

	case WEST:
		glVertex2f(x - (halfLength + e), y);
		glVertex2f(x + (halfBreadth + e), 
			   y - (halfBreadth + e));
		glVertex2f(x + (halfBreadth + e), 
			   y + (halfBreadth + e));
		break;
	}

	glEnd();
	glPopMatrix();
}

static GLfloat getArea(vector<TOCGhoul> points)
{
	GLfloat areaTimes2 = 0.0;
	const int numPoints = points.size();
	points.push_back(points[0]); // Closed polygon
	for(int i = 0; i < numPoints; i++) {
		areaTimes2 += ((points[i].getX() * points[i + 1].getY()) 
			       - (points[i + 1].getX() * points[i].getY()));
	}

	return (areaTimes2 / 2);
}

vector<float> getCentroid(vector<TOCGhoul> &neighbors)
{
	vector<float> centroid;
	GLfloat aggX = 0.0;
	GLfloat aggY = 0.0;

	const int N = neighbors.size();

	if (N == 1) {
		centroid.push_back(neighbors[0].getX()); // x
		centroid.push_back(neighbors[0].getY()); // y
		return centroid;
	}

	GLfloat A = getArea(neighbors);
	// /* assert(A != 0); */

	if (A == 0.0)
		A = 1.0;


	centroid.push_back(0.0f); // x
	centroid.push_back(0.0f); // y

	neighbors.push_back(neighbors[0]); // Closed polygon

	for (int i = 0; i < N; i++) {
		const GLfloat commonTerm  = (neighbors[i].getX() * neighbors[i + 1].getY()) 
			- (neighbors[i + 1].getX() * neighbors[i].getY());
		aggX += (neighbors[i].getX() + neighbors[i + 1].getX()) * commonTerm;
		aggY += (neighbors[i].getY() + neighbors[i + 1].getY()) * commonTerm;
	}

	centroid[0] = aggX / (6 * A);
	centroid[1] = aggY / (6 * A);

	return centroid;
}
